#include "lqr_vmc_controller/lqr_vmc.hpp"

#include "rclcpp/rclcpp.hpp"

namespace lqr_vmc_controller
{
lqr_vmc::lqr_vmc()
: update_rate_{200.0},
  robot_displacement_{0.0},
  robot_pitch_{0.0},
  robot_velocity_{0.0},
  robot_pitchome_{0.0},
  robot_yaw_{0.0},
  robot_yawome_{0.0},
  robot_thigh_position_offset_{M_PI*62.0/180.0},
  robot_m_{38.376},
  wheel_separation_{0.568},

  L_11_{0.432},
  L_12_{0.185},
  L_2_{0.382},
  L_3_{0.382},
  L_4_{0.317},
  L_error_integral_right_{0.0},
  L_error_integral_left_{0.0},
  L_error_previous_right_{0.0},
  L_error_previous_left_{0.0},
  L_error_dot_right_previous_{0.0},
  L_error_dot_left_previous_{0.0},
  roll_error_previous_{0.0},
  roll_error_intergral_{0.0},
  roll_error_dot_previous_{0.0}
{
}

void lqr_vmc::setlqrfeedback(double robot_displacement, double robot_pitch, double robot_velocity, double robot_pitchome, double robot_yaw, double robot_yawome)
{
    robot_displacement_ = robot_displacement;
    robot_pitch_ = robot_pitch;
    robot_velocity_ = robot_velocity;
    robot_pitchome_ = robot_pitchome;
    robot_yaw_ = robot_yaw;
    robot_yawome_ = robot_yawome;
}

Eigen::Matrix<double, 2 ,1> lqr_vmc::lqr(double right_thigh_theta, double left_thigh_theta)
{
    double theta = (right_thigh_theta + left_thigh_theta)/2 + robot_thigh_position_offset_; 
    // 计算theta的各个次方
    double theta_fivepow = pow(theta, 5);
    double theta_fourpow = pow(theta, 4);
    double theta_threepow = pow(theta, 3);
    double theta_twopow = pow(theta, 2);
    // 各拟合多项式系数的值
    std::vector<double> p_11 = {1.31716039355272e-10,	-8.72013782539459e-10,	2.28843266684282e-09,	-2.97587729124603e-09,	1.91768207459934e-09,	-10.0000000004899};
    std::vector<double> p_12 = {-161.218196791945,		1142.71667836818,		-3236.28878097393,	4589.05942710771,		-3285.00276145431,	864.621959600265};
    std::vector<double> p_13 = {11.9017126157661,		-85.8064490203385,	247.767659965897,		-359.218828217407,	262.678735948928,		-92.2807882569980};
    std::vector<double> p_14 = {-20.4703372014503,		140.429793446154,		-381.993642200172,	514.227972755231,		-347.421376048275,	72.1015294267859};
    std::vector<double> p_15 = {4.25364030732405e-10,	-2.89264386008655e-09,	7.81907820844282e-09,	-1.04999177395159e-08,	7.00401146652530e-09,	-24.4948974296884};
    std::vector<double> p_16 = {0.0536605643588279,		-0.399043593093022,	1.01512773408596,		-0.866029040165660,	0.228072015555706,	-19.1820633493041};
    std::vector<double> p_21 = {1.23491200320153e-10,	-8.22176632439233e-10,	2.17050355118221e-09,	-2.84069666943186e-09,	1.84362730251546e-09,	-10.0000000004748};
    std::vector<double> p_22 = {-161.218196792061,		1142.71667836896,		-3236.28878097600,	4589.05942711046,		-3285.00276145613,	864.621959600739};
    std::vector<double> p_23 = {11.9017126157691,		-85.8064490203671,	247.767659965997,		-359.218828217573,	262.678735949061,		-92.2807882570396};
    std::vector<double> p_24 = {-20.4703372014878,		140.429793446409,		-381.993642200860,	514.227972756159,		-347.421376048898,	72.1015294269526};
    std::vector<double> p_25 = {-1.49165793612347e-10,	1.02141394932778e-09,	-2.77947450208839e-09,	3.75587730200111e-09,	-2.51970885841884e-09,	24.4948974285031};
    std::vector<double> p_26 = {-0.0536605642886277,	0.399043592602724,	-1.01512773272386,	0.866029038284482,	-0.228072014264229,	19.1820633489515}; 
    // 计算增益矩阵的各元素值
    double K_11 = p_11[0]*theta_fivepow + p_11[1]*theta_fourpow + p_11[2]*theta_threepow + p_11[3]*theta_twopow + p_11[4]*theta + p_11[5];
    double K_12 = p_12[0]*theta_fivepow + p_12[1]*theta_fourpow + p_12[2]*theta_threepow + p_12[3]*theta_twopow + p_12[4]*theta + p_12[5];
    double K_13 = p_13[0]*theta_fivepow + p_13[1]*theta_fourpow + p_13[2]*theta_threepow + p_13[3]*theta_twopow + p_13[4]*theta + p_13[5];
    double K_14 = p_14[0]*theta_fivepow + p_14[1]*theta_fourpow + p_14[2]*theta_threepow + p_14[3]*theta_twopow + p_14[4]*theta + p_14[5];
    double K_15 = p_15[0]*theta_fivepow + p_15[1]*theta_fourpow + p_15[2]*theta_threepow + p_15[3]*theta_twopow + p_15[4]*theta + p_15[5];
    double K_16 = p_16[0]*theta_fivepow + p_16[1]*theta_fourpow + p_16[2]*theta_threepow + p_16[3]*theta_twopow + p_16[4]*theta + p_16[5];
    double K_21 = p_21[0]*theta_fivepow + p_21[1]*theta_fourpow + p_21[2]*theta_threepow + p_21[3]*theta_twopow + p_21[4]*theta + p_21[5];
    double K_22 = p_22[0]*theta_fivepow + p_22[1]*theta_fourpow + p_22[2]*theta_threepow + p_22[3]*theta_twopow + p_22[4]*theta + p_22[5];
    double K_23 = p_23[0]*theta_fivepow + p_23[1]*theta_fourpow + p_23[2]*theta_threepow + p_23[3]*theta_twopow + p_23[4]*theta + p_23[5];
    double K_24 = p_24[0]*theta_fivepow + p_24[1]*theta_fourpow + p_24[2]*theta_threepow + p_24[3]*theta_twopow + p_24[4]*theta + p_24[5];
    double K_25 = p_25[0]*theta_fivepow + p_25[1]*theta_fourpow + p_25[2]*theta_threepow + p_25[3]*theta_twopow + p_25[4]*theta + p_25[5];
    double K_26 = p_26[0]*theta_fivepow + p_26[1]*theta_fourpow + p_26[2]*theta_threepow + p_26[3]*theta_twopow + p_26[4]*theta + p_26[5];
    // 求解车轮扭矩
    Eigen::Matrix<double, 2 ,6>  K;
    K << K_11, K_12, K_13, K_14, K_15, K_16,
         K_21, K_22, K_23, K_24, K_25, K_26;
    Eigen::Matrix<double, 6, 1> x;
    x << robot_displacement_,
         robot_pitch_,
         robot_velocity_,
         robot_pitchome_,
         robot_yaw_,
         robot_yawome_;
    Eigen::Matrix<double, 2, 1> wheels_effor;
    wheels_effor = -K*x;
    return wheels_effor;

}

double lqr_vmc::vmc_right(double thigh_angle_d, double thigh_angle, double roll_PD_L, double roll_d, double roll)
{
    // PID参数
    double K_p = 4000.0;
    double K_i = 0.0;
    double K_d = 300.0;  // 实物参数
    // double K_p = 1500;
    // double K_i = 0.0;
    // double K_d = 300; // 仿真参数

    // 添加角度偏置
    thigh_angle_d += robot_thigh_position_offset_;
    thigh_angle += robot_thigh_position_offset_;
    // 计算thigh_angle的各个次方
    double thighangle_fivepow = pow(thigh_angle, 5);
    double thighangle_fourpow = pow(thigh_angle, 4);
    double thighangle_threepow = pow(thigh_angle, 3);
    double thighangle_twopow = pow(thigh_angle, 2);
    // 计算thigh_angle_d的各个次方
    double thighangle_d_fivepow = pow(thigh_angle_d, 5);
    double thighangle_d_fourpow = pow(thigh_angle_d, 4);
    double thighangle_d_threepow = pow(thigh_angle_d, 3);
    double thighangle_d_twopow = pow(thigh_angle_d, 2);
    // 对theta1_d和theta1进行多项式拟合结果计算
    // theta1多项式拟合系数
    std::vector<double> p_theta1 = {1.13999750994618,	-7.40701633251438,	19.7279210315040,	-26.9247204354692,       19.5053835208596,	-5.55745452876576};
    double theta1 = p_theta1[0]*thighangle_fivepow + p_theta1[1]*thighangle_fourpow+ p_theta1[2]*thighangle_threepow + p_theta1[3]*thighangle_twopow+ p_theta1[4]*thigh_angle + p_theta1[5];
    double theta1_d = p_theta1[0]*thighangle_d_fivepow + p_theta1[1]*thighangle_d_fourpow+ p_theta1[2]*thighangle_d_threepow + p_theta1[3]*thighangle_d_twopow+ p_theta1[4]*thigh_angle_d + p_theta1[5];
    double L_d = L_2_*sin(thigh_angle_d - M_PI/4) + L_11_*sin(theta1_d);
    double L = L_2_*sin(thigh_angle - M_PI/4) + L_11_*sin(theta1);

    // Ld 结合roll轴前馈
    L_d -= (wheel_separation_ + 0.0)*tan(roll_d- roll)*0.5; // 添加roll前馈误差补偿
    // 计算误差
    double L_error = L_d - L + roll_PD_L;
    double L_error_dot = (L_error - L_error_previous_right_)/(1.0/update_rate_);
    // 对L_error_dot进行一阶低通滤波
    double a = 0.03; // 滤波系数
    L_error_dot = a*L_error_dot + (1 - a)*L_error_dot_right_previous_;
    L_error_dot_right_previous_ = L_error_dot;

    // 前馈重力
    double G = 0.5*robot_m_*9.8 - 42;  // 前馈误差补偿 // 仿真补偿为 +22

    // PID得到输出弹簧力 + 重力前馈
    double F_spring = K_p*L_error + K_i*L_error_integral_right_ + K_d*L_error_dot + G;
    L_error_previous_right_ = L_error;
    L_error_integral_right_ += L_error*(1.0/update_rate_);

    /* vmc力解算 */ 
    double alpha = 45.0*M_PI/180.0;
    double theta_2 = M_PI + alpha - thigh_angle;
    double theta_0 = acos((pow(L_4_, 2) + pow(L_3_, 2) - (pow(L_2_, 2) + pow(L_12_, 2) - 2*L_2_*L_12_*cos(theta_2 - theta1)))/(2*L_4_*L_3_));
    double theta_3 = theta_0 + thigh_angle + theta_2 - M_PI;
    double X = L_12_*(tan(theta_3 - M_PI/2)*tan(theta1)+1);
    // 扭簧部分前馈
    double theta_i = 120*M_PI/180.0;
    double k = 20.79236; // 扭簧刚度系数
    double torque_sp = k*(theta_i - M_PI + theta_2 - theta1);
    double TorqueL = F_spring*L_2_*((sin(theta_2-M_PI/2)*(1+L_11_/X))-((L_11_*tan(theta_3-M_PI/2)*cos(theta_2-M_PI/2))/X))
                    + ((torque_sp*L_2_)/(cos(theta1)*X))*(-sin(theta_2 - M_PI/2) + tan(theta_3-M_PI/2)*cos(theta_2-M_PI/2))
                    - torque_sp;

    return TorqueL;
}

double lqr_vmc::vmc_left(double thigh_angle_d, double thigh_angle, double roll_PD_L, double roll_d, double roll)
{
    // PID参数
    double K_p = 4000.0;
    double K_i = 0.0;
    double K_d = 300.0;  // 实物参数
    // double K_p = 1500;
    // double K_i = 0.0;
    // double K_d = 300;    // 仿真参数

    // 添加角度偏置
    thigh_angle_d += robot_thigh_position_offset_;
    thigh_angle += robot_thigh_position_offset_;
    // 计算thegh_angle的各个次方
    double thighangle_fivepow = pow(thigh_angle, 5);
    double thighangle_fourpow = pow(thigh_angle, 4);
    double thighangle_threepow = pow(thigh_angle, 3);
    double thighangle_twopow = pow(thigh_angle, 2);
    // 计算thigh_angle_d的各个次方
    double thighangle_d_fivepow = pow(thigh_angle_d, 5);
    double thighangle_d_fourpow = pow(thigh_angle_d, 4);
    double thighangle_d_threepow = pow(thigh_angle_d, 3);
    double thighangle_d_twopow = pow(thigh_angle_d, 2);
    // 对theta1_d和theta1进行多项式拟合结果计算
    // theta1多项式拟合系数
    std::vector<double> p_theta1 = {1.13999750994618,	-7.40701633251438,	19.7279210315040,	-26.9247204354692,       19.5053835208596,	-5.55745452876576};
    double theta1 = p_theta1[0]*thighangle_fivepow + p_theta1[1]*thighangle_fourpow+ p_theta1[2]*thighangle_threepow + p_theta1[3]*thighangle_twopow+ p_theta1[4]*thigh_angle + p_theta1[5];
    double theta1_d = p_theta1[0]*thighangle_d_fivepow + p_theta1[1]*thighangle_d_fourpow+ p_theta1[2]*thighangle_d_threepow + p_theta1[3]*thighangle_d_twopow+ p_theta1[4]*thigh_angle_d + p_theta1[5];
    double L_d = L_2_*sin(thigh_angle_d - M_PI/4) + L_11_*sin(theta1_d);
    double L = L_2_*sin(thigh_angle - M_PI/4) + L_11_*sin(theta1);

    // Ld 结合roll轴前馈
    L_d += (wheel_separation_ + 0.0)*tan(roll_d- roll)*0.5; // 添加roll前馈误差补偿
    // 计算误差
    double L_error = L_d - L + roll_PD_L;
    double L_error_dot = (L_error - L_error_previous_left_)/(1.0/update_rate_);
    // 对L_error_dot进行一阶低通滤波
    double a = 0.03;
    L_error_dot = a*L_error_dot + (1 - a)*L_error_dot_left_previous_;
    L_error_dot_left_previous_ = L_error_dot;

    // 前馈重力
    double G = 0.5*robot_m_*9.8 - 42; // 前馈误差补偿  // 仿真补偿为 + 22

    // PID得到输出力 + 重力前馈
    double F_spring = K_p*L_error + K_i*L_error_integral_left_ + K_d*L_error_dot + roll_PD_L + G;
    L_error_previous_left_ = L_error;
    L_error_integral_left_ += L_error*(1.0/update_rate_);

    // vmc力解算
    double alpha = 45.0*M_PI/180.0;
    double theta_2 = M_PI + alpha - thigh_angle;
    double theta_0 = acos((pow(L_4_, 2) + pow(L_3_, 2) - (pow(L_2_, 2) + pow(L_12_, 2) - 2*L_2_*L_12_*cos(theta_2 - theta1)))/(2*L_4_*L_3_));
    double theta_3 = theta_0 + thigh_angle + theta_2 - M_PI;
    double X = L_12_*(tan(theta_3 - M_PI/2)*tan(theta1)+1);
    // 扭簧部分前馈
    double theta_i = 120*M_PI/180.0;
    double k = 20.79236; // 扭簧刚度系数
    double torque_sp = k*(theta_i - M_PI + theta_2 - theta1);
    double TorqueL = F_spring*L_2_*((sin(theta_2-M_PI/2)*(1+L_11_/X))-((L_11_*tan(theta_3-M_PI/2)*cos(theta_2-M_PI/2))/X))
                    + ((torque_sp*L_2_)/(cos(theta1)*X))*(-sin(theta_2 - M_PI/2) + tan(theta_3-M_PI/2)*cos(theta_2-M_PI/2))
                    - torque_sp;
    return TorqueL;
}

double lqr_vmc::roll_PD(double roll_angle_d, double roll_angle)
{
    // PD参数
    double K_p = 0.52;
    double K_i = 0.0;
    double K_d = 0.0;  // 实物参数
    // 仿真参数
    // double K_p = 1.0;
    // double K_i = 0.0;
    // double K_d = 1.0; 

    // 计算误差
    double roll_error = roll_angle_d - roll_angle;
    double roll_error_dot = (roll_error - roll_error_previous_)/(1.0/update_rate_);
    // 一阶低通滤波
    double a = 0.02;
    roll_error_dot = a*roll_error_dot + (1 - a)*roll_error_dot_previous_;
    roll_error_dot_previous_ = roll_error_dot;

    double roll_PD_effort = K_p*roll_error + K_i*roll_error_intergral_ + K_d*roll_error_dot;
    roll_error_previous_ = roll_error;
    roll_error_intergral_ += roll_error*(1.0/update_rate_);

    return roll_PD_effort;

}

double lqr_vmc::initial_pitch(double right_thigh_angle, double left_thigh_angle)
{
    double thigh_angle = (right_thigh_angle + left_thigh_angle)/2;
    // 添加角度偏置
    thigh_angle += robot_thigh_position_offset_;
    // 计算thigh_angle的各个次方
    double thigh_angle_fivepow = pow(thigh_angle, 5);
    double thigh_angle_fourpow = pow(thigh_angle, 4);
    double thigh_angle_threepow = pow(thigh_angle, 3);
    double thigh_angle_twopow = pow(thigh_angle, 2);
    // initial_pitch多项式拟合系数
    std::vector<double> p_pitch = {1.26265047636111,	-8.87490846591352,	25.4960231332494,	-37.4766582227709,	28.2827776243294,	-8.80852423426532};
    double initial_pitch = p_pitch[0]*thigh_angle_fivepow + p_pitch[1]*thigh_angle_fourpow + p_pitch[2]*thigh_angle_threepow + p_pitch[3]*thigh_angle_twopow + p_pitch[4]*thigh_angle + p_pitch[5];
    return initial_pitch;
}
}